#ifndef DRIVABLE_SEG_H_
#define DRIVABLE_SEG_H_

#include <ros/ros.h>
#include <opencv/cv.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/common/common.h>
#include <opencv2/imgproc.hpp>
#include "glog/logging.h"
#include <vector>
#include <cmath>
#include <geometry_msgs/Pose.h>
#include <sensor_msgs/PointCloud2.h>
#include <DrivableSegUtility.h>
#include <pcl/filters/crop_box.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include "GroundPlaneFit.h"
//    API 1:ComputeDrivableArea(const cv::Mat &range_mat,pcl::PointCloud<PointType>::Ptr InputfullCloud,
//                              pcl::PointCloud<PointType>::Ptr ground_cloud,cv::Mat &ground_mat)
//    输入深度图和其对应的点云，输出地面点云，地面点的标记(仅适用于LeGO-LOAM)
//    API 2:ComputeDrivableArea(const cv::Mat &range_mat,pcl::PointCloud<PointType>::Ptr ground_cloud)
//    输入深度图，深度图中没有深度的点为FLT_MAX，输出地面点云
//    API 3:ComputeDrivableArea(pcl::PointCloud<PointType>::ConstPtr laserCloudIn,pcl::PointCloud<PointType>::Ptr ground_cloud)
//    输入原始点云，输出地面点云
//
//    cv::Mat GetGroundMat()获取地面标记
//    cv::Mat GetAngleImage();获取角度图
//    cv::Mat GetRangeMat();获取深度图
using namespace std;
namespace ObstacleCluster {
    class DrivableSeg {

        typedef pcl::PointXYZI PointType;

        struct PointXYZIR {
            PCL_ADD_POINT4D

            PCL_ADD_INTENSITY;
            uint16_t ring;

            EIGEN_MAKE_ALIGNED_OPERATOR_NEW
        } EIGEN_ALIGN16;

    public:
        DrivableSeg(ros::NodeHandle &nh);

        ~DrivableSeg() {};

        void ApplySavitskyGolaySmoothing(cv::Mat &angle_image, int window_size);

        cv::Mat ApplyUniformSmoothing(cv::Mat angle_image, int window_size);

        cv::Mat GetSavitskyGolayKernel(int window_size) const;

        cv::Mat GetUniformKernel(int window_size, int type = CV_32F) const;

        void Label_drivable_area(pcl::PointCloud<pcl::PointXYZI> &fullCloud,
                                 pcl::PointCloud<PointType> &ground_cloud, cv::Mat &ground_mat);

        void RangeToPointCloud(const cv::Mat &range_mat);

        void ComputeScanAngle(const cv::Mat &range_mat);

        void ComputeRangeMat(const pcl::PointCloud<PointType>::ConstPtr InputCloud, cv::Mat &range_mat);

        void ComputeDrivableArea(const cv::Mat &rangeMat, pcl::PointCloud<PointType>::Ptr fullCloud,
                                 pcl::PointCloud<PointType>::Ptr ground_cloud, cv::Mat &ground_mat);

        void ComputeDrivableArea(const cv::Mat &rangeMat,
                                 pcl::PointCloud<PointType>::Ptr ground_cloud);

        void ComputeDrivableArea(pcl::PointCloud<PointType>::ConstPtr fullCloud,
                                 pcl::PointCloud<PointType>::Ptr ground_cloud);

        cv::Mat GetAngleImage();

        cv::Mat GetRangeMat();

        cv::Mat GetGroundMat();

        void Filter(cv::Mat &angle_img);

        void extract_object();

        void labelComponents(int row, int col);

        pcl::PointCloud<pcl::PointXYZI>::Ptr GetSegObjCloud();

        void lidarCallback(const sensor_msgs::PointCloud2ConstPtr &lidar_ptr);

        void copyPointCloud(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg);

        void PublishCloud();

        void gpf_label_drivabel_area(pcl::PointCloud<pcl::PointXYZI>::Ptr& g_ground_pc);

    private:
        ros::Subscriber sub_lidarcloud;

        ros::Publisher pub_segmented_obj;
        ros::Publisher pub_ground;
        ros::Publisher pub_reversed_zvision_lidar;

        std_msgs::Header cloudHeader;

        vector<float> row_angle_sin;
        vector<float> row_angle_cos;

        cv::Mat angle_image;
        cv::Mat x_mat;
        cv::Mat y_mat;
        cv::Mat temp_ground_Mat;
        cv::Mat rangeMat;
        cv::Mat groundMat;
        cv::Mat labelMat;

        PointType nanPoint;

        pcl::PointCloud<PointType>::Ptr fullPointCloud;
        pcl::PointCloud<PointType>::Ptr SegObjectCloud;
        pcl::PointCloud<PointType>::Ptr GroundCloud;
        pcl::PointCloud<PointType>::Ptr ProjectedPointCloud;

        std::vector<std::pair<int8_t, int8_t> > neighborIterator; // neighbor iterator for segmentaiton process

        uint16_t *allPushedIndX; // array for tracking points of a segmented object
        uint16_t *allPushedIndY;

        uint16_t *queueIndX; // array for breadth-first search process of segmentation, for speed
        uint16_t *queueIndY;

        float dx, dy;
        float threshold;

        int labelCount;

        //gpf参数
        std::shared_ptr<GroundPlaneFit> gpfmethod;
        Eigen::MatrixXf normal_out;
        float th_dist_d_out;

        pcl::CropBox<PointType> boundary_box_filter_;
        pcl::VoxelGrid<PointType> Voxel_box_filter_;
        pcl::StatisticalOutlierRemoval<pcl::PointXYZ> Outlier_box_filter_;

    };

}

#endif
